#include "track/tracker.h"

#include "common/utility.h"
#include "track/file_helper.h"
#include "track/post_processor.h"
#include "track/settings.h"

namespace autodrive {
namespace perception {
Tracker::Tracker()
    : turn_processor(std::make_unique<Postprocessor>()),
      crosswalk_processor(std::make_unique<Postprocessor>()),
      nostop_processor(std::make_unique<Postprocessor>()),
      settings_ptr_(std::make_shared<MapTrackParameter>()) {}

Tracker::~Tracker() {}

void Tracker::Init(const std::string& config_path) {
  // process config file
  bool b_ret{true};
  std::string err_msg;
  // create config path
  if (!IsFileExist(config_path)) {
    b_ret = CreateDirectory(config_path, err_msg);
    if (!b_ret) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "create directory  [ " << config_path << " ] fail !"
                << std::endl;
    }
  }
  std::string configFileName{"track_conf.json"};

  auto const& configFile{
      boost::filesystem::complete(configFileName, config_path)};
  if (IsFileExist(configFile.string())) {
    ReadTrackParameter(configFile.string(), *settings_ptr_);
    // std::cout << "param : " << settings_.turn_score_threshold << std::endl;
  } else {
    settings_ptr_->turn_score_threshold = 0.51f;
    settings_ptr_->cross_score_threshold = 0.4f;
    settings_ptr_->nostop_score_threshold = 0.4f;
    settings_ptr_->DBSCAN_eps_turn = 1.0f;
    settings_ptr_->DBSCAN_min_num_turn = 4;
    settings_ptr_->DBSCAN_eps_crosswalk = 1.0f;
    settings_ptr_->DBSCAN_min_num_crosswalk = 4;
    settings_ptr_->DBSCAN_eps_nostop = 1.0f;
    settings_ptr_->DBSCAN_min_num_nostop = 4;
    settings_ptr_->s_match_2DIOU_minimum = 0.1f;
    settings_ptr_->s_match_distance_maximum = 1.8f;
    settings_ptr_->collect_track_consecutive_invisible_maximum = 4;
    settings_ptr_->collect_track_age_minimum = 5;
    settings_ptr_->position_filter_alpha = 0.5f;
    settings_ptr_->length_filter_alpha = 0.5f;
    settings_ptr_->width_filter_alpha = 0.5f;
    settings_ptr_->yaw_filter_alpha = 0.5f;
    WriteTrackParameter(configFile.string(), *settings_ptr_);
  }

  // processor init
  turn_processor->Init(*settings_ptr_, ProcessCategory::Turn);
  crosswalk_processor->Init(*settings_ptr_, ProcessCategory::CrossWalk);
  nostop_processor->Init(*settings_ptr_, ProcessCategory::No_Stop);
}

void Tracker::Run(autodrive::perception::Map& map, const Eigen::Affine3d& pos,
                  RoadMarkingResultSet& output_roadmarking) {
  float turn_score_threshold =
      settings_ptr_->turn_score_threshold;  // score的阈值
  float cross_score_threshold =
      settings_ptr_->cross_score_threshold;  // score的阈值
  float nostop_score_threshold =
      settings_ptr_->nostop_score_threshold;  // score的阈值
  std::vector<Eigen::Vector2f> turn_points;
  std::vector<Eigen::Vector2f> crosswalk_points;
  std::vector<Eigen::Vector2f> nostop_points;

  extract_map_roadmarking_data(map, turn_points, crosswalk_points,
                               nostop_points, turn_score_threshold,
                               cross_score_threshold, nostop_score_threshold);

  // process
  output_roadmarking.road_markings.clear();
  // turn process
  turn_processor->Process(map, output_roadmarking.road_markings, turn_points,
                          pos, *settings_ptr_);
  // crosswalk process
  crosswalk_processor->Process(map, output_roadmarking.road_markings,
                               crosswalk_points, pos, *settings_ptr_);
  // nostop process
  nostop_processor->Process(map, output_roadmarking.road_markings,
                            nostop_points, pos, *settings_ptr_);
}
}  // namespace perception
}  // namespace autodrive
